/*
 * syndet.c
 *
 *  Created on: Oct 24, 2019
 *      Author: Standa
 */

#include "syndet.h"
#include "main.h"
#include "utility.h"
#include "error.h"
#include <sin128.h>
#include "generator.h"
#include "measurement.h"
#include "calibration.h"
#include "qfplib.h"


#define SD_FMMIN_HZ					0.1					//Minimum measurement frequency FM
#define SD_FMMAX_HZ					1000				//Maximum measurement frequency FM

#define CLIPTYPESCNT				6
#define CLIPVM100LOW				0
#define CLIPVM100HIGH				1
#define CLIPIMLOW					2
#define CLIPIMHIGH					3
#define CLIPVM10LOW					4
#define CLIPVM10HIGH				5

#define CLIPTRESHOLDLOW				200
#define CLIPTRESHOLDHIGH			3895
#define CLIPTRESHOLDRATIO			0.01		//Percentage of allowed cycles with voltage clips for all the measurement cycles, used to select proper voltage range
#define COVTRESHOLDRATIO			0.05		//Allowed coefficient of variance (100*stdev/mean)
#define LOWVOLTAGETRESHOLDRATIO		0.00		//If the amplitude of sensed voltage is lower than x percent of range - disabled to allow short circuit measurement without FMI
#define LOWCURRENTTRESHOLDRATIO		0.02		//If the amplitude of sensed current is lower than x percent of range

#define FMIBITPOSVOLTLOWCLIP		0		//Voltage reading low clip happened
#define FMIBITPOSVOLTHIGHCLIP		1		//Voltage reading high clip happened
#define FMIBITPOSVOLTLOWVAL			2		//Voltage reading signal is too low
#define FMIBITPOSVOLTCOVHIGH		3		//Voltage reading coefficient of variance is too high
#define FMIBITPOSCURLOWCLIP			4		//Current reading low clip happened
#define FMIBITPOSCURHIGHCLIP		5		//Current reading high clip happened
#define FMIBITPOSCURLOWVAL			6		//Current reading signal is too low
#define FMIBITPOSCURCOVHIGH			7		//Current reading coefficient of variance is too high
#define FMIBITPOSZOVERRANGE			8		//Measured impedance is too high (over range)
#define FMIBITPOSZCOVHIGH			9		//Coefficient of variance for measured impedance is too high


static float VRef_V = 0;


static volatile uint8_t MeasurementActiveFlag = 0, DesiredCycles = 0, DropCycles = 0, FinishedCycles = 0, LatestEvaluatedCycle = 0;
static float Freq_Hz = 0;


ut_MeanStdevTd VM10R, VMFi10R, IM, IMFi, VM100R, VMFi100R;			//Statistics over all measurement cycles except for drop cycles
ut_MeanStdevTd Z10R, ZFi10R, Z100R, ZFi100R;
ut_MeanStdevTd R10R, X10R, R100R, X100R;
ut_MeanStdevTd DF10R, Q10R, CS10R, LS10R, DF100R, Q100R, CS100R, LS100R;

ut_MeanStdevTd Clip10RLow, Clip10RHigh;
ut_MeanStdevTd Clip100RLow, Clip100RHigh;
ut_MeanStdevTd ClipIMLow, ClipIMHigh;


static sd_Results Res10R = {0, 0, &VM10R, &VMFi10R, &IM, &IMFi, &Z10R, &ZFi10R, &R10R, &X10R, &CS10R, &LS10R, &DF10R, &Q10R, &Clip10RLow, &Clip10RHigh, &ClipIMLow, &ClipIMHigh};
static sd_Results Res100R = {0, 0, &VM100R, &VMFi100R, &IM, &IMFi, &Z100R, &ZFi100R, &R100R, &X100R, &CS100R, &LS100R, &DF100R, &Q100R, &Clip100RLow, &Clip100RHigh, &ClipIMLow, &ClipIMHigh};

static ut_MeanStdevTd* Statistics[] = {&VM10R, &VMFi10R, &IM, &IMFi, &VM100R, &VMFi100R, &Z10R, &ZFi10R, &Z100R, &ZFi100R, &R10R, &X10R, &R100R, &X100R,
		&DF10R, &Q10R, &CS10R, &LS10R, &DF100R, &Q100R, &CS100R, &LS100R, &Clip10RLow, &Clip10RHigh, &Clip100RLow, &Clip100RHigh, &ClipIMLow, &ClipIMHigh};


void UpdateFMIandFreq(sd_Results* res){
	if(!res) return;

	uint16_t Range;
	if(res == &Res10R) Range = 10;
	else if(res == &Res100R) Range = 100;
	else Range = 1;		//Just invalid number to kind of force fmis to appear, infamous "this should never happen" case

	res->FMI = 0;

	if( qfp_fdiv(res->ClipVlow->Mean, res->ClipVlow->Cnt ) > CLIPTRESHOLDRATIO) res->FMI |= 1<<FMIBITPOSVOLTLOWCLIP;
	if( qfp_fdiv(res->ClipVhigh->Mean, res->ClipVhigh->Cnt ) > CLIPTRESHOLDRATIO) res->FMI |= 1<<FMIBITPOSVOLTHIGHCLIP;
	//if( qfp_fdiv(res->V->Mean, CAL_ADCRANGE ) < LOWVOLTAGETRESHOLDRATIO) res->FMI |= 1<<FMIBITPOSVOLTLOWVAL;						//100x is conversion to percent, 2x is conversion from amplitude to peaktopeak value
	//if( qfp_fdiv( res->V->Stdev, res->V->Mean ) > COVTRESHOLDRATIO) res->FMI |= 1<<FMIBITPOSVOLTCOVHIGH;

	if( qfp_fdiv(res->ClipIlow->Mean, res->ClipIlow->Cnt ) > CLIPTRESHOLDRATIO) res->FMI |= 1<<FMIBITPOSCURLOWCLIP;
	if( qfp_fdiv(res->ClipIhigh->Mean, res->ClipIhigh->Cnt ) > CLIPTRESHOLDRATIO) res->FMI |= 1<<FMIBITPOSCURHIGHCLIP;
	if( qfp_fdiv(res->I->Mean, MEAS_ADCRANGE ) < LOWCURRENTTRESHOLDRATIO) res->FMI |= 1<<FMIBITPOSCURLOWVAL;						//100x is conversion to percent, 2x is conversion from amplitude to peaktopeak value
	//if( qfp_fdiv(res->I->Stdev, res->I->Mean ) > COVTRESHOLDPERC) res->FMI |= 1<<FMIBITPOSCURCOVHIGH;

	if( res->Z->Mean > 5*Range ) res->FMI |= 1<<FMIBITPOSZOVERRANGE;
	if( qfp_fdiv(res->Z->Stdev, Range ) > COVTRESHOLDRATIO) res->FMI |= 1<<FMIBITPOSZCOVHIGH;

	res->Freq_Hz = sd_GetMeasFreq();
}


/*
 * Gets called when single measurement cycle is ready (by ISR of DMA1 transfer complete)
 */
static void MeasurementCycleFinishedHook(void){
	FinishedCycles++;
}


/*
 * Updates Vref based on reference voltage calibration
 */
static void UpdateVRef(void){
	VRef_V = CAL_VREF_25DEG_V;
}

/*
 * Initializes synchronous detection module
 */
void sd_init(void){
	UpdateVRef();
}


/*
 * Stops the ongoing measurement (output excitation and reading)
 */
void sd_StopMeasurement(void){
	gen_Stop();						//Disable the waveform generator module
	meas_Stop();					//Disable the measurement module
	meas_SetCycleFinishedHook(0);	//Remove the hook
	MeasurementActiveFlag = 0;
}


/*
 * Checks whether specified conversion value exceeds minimum or maximum limit (defined by macros) and if yes increments respective counter
 */
void CheckConvSaturation(uint16_t val, uint8_t* minCntr, uint8_t* maxCntr){
	if( val < CLIPTRESHOLDLOW && *minCntr < 0xFF ) (*minCntr)++; \
	else if( val > CLIPTRESHOLDHIGH && *maxCntr < 0xFF ) (*maxCntr)++;\
}

float CalcMagnitude(float x, float y){
	return qfp_fsqrt( qfp_fadd( qfp_fmul(x, x), qfp_fmul(y, y) ) );
}



/*
 * Evaluate the finished measurement
 */
static void EvaluateMeasurement(uint8_t dropResultsFlag){
	float SinVal, CosVal;
	float VM100R_X = 0, VM100R_Y = 0, VM10R_X = 0, VM10R_Y = 0, IM_X = 0, IM_Y = 0;			//Latest evaluated measurement values
	uint8_t FMI[CLIPTYPESCNT];										//Latest evaluated measurement FMI values

	ut_CplxTd VM10Cplx, VM100Cplx, IMCplx, Z10Cplx, Z100Cplx, Z10CompCplx, Z100CompCplx;

	uint16_t* FullBuffer = meas_GetDataBuffer();

	if(dropResultsFlag) return;

	if(FinishedCycles == 1){
		for(uint16_t i=0; i<UT_SIZEOFARRAY(Statistics); i++){
			ut_InitializeOnlineMeanStdev( Statistics[i] );
		}
	}

	for(uint16_t i=0; i<CLIPTYPESCNT; i++) FMI[i] = 0;


	for(uint16_t i=0; i<3*MEAS_READSPERMEASPERIOD; i+=3){			//Synchronous detection algorithm
		SinVal = Sin128[i/3];
		CosVal = Sin128[0x7F & (SIN128_CNT/4 + i/3)];

		VM100R_X = qfp_fadd(VM100R_X, qfp_fmul(FullBuffer[i+0], CosVal));
		VM100R_Y = qfp_fadd(VM100R_Y, qfp_fmul(FullBuffer[i+0], SinVal));
		CheckConvSaturation(FullBuffer[i+0], FMI + CLIPVM100LOW, FMI + CLIPVM100HIGH);

		VM10R_X = qfp_fadd(VM10R_X, qfp_fmul(FullBuffer[i+2], CosVal));
		VM10R_Y = qfp_fadd(VM10R_Y, qfp_fmul(FullBuffer[i+2], SinVal));
		CheckConvSaturation(FullBuffer[i+2], FMI + CLIPVM10LOW, FMI + CLIPVM10HIGH);

		IM_X = qfp_fadd(IM_X, qfp_fmul(FullBuffer[i+1], CosVal));
		IM_Y = qfp_fadd(IM_Y, qfp_fmul(FullBuffer[i+1], SinVal));
		CheckConvSaturation(FullBuffer[i+1], FMI + CLIPIMLOW, FMI + CLIPIMHIGH);
	}

	//VOLTAGE AND CURRENT CALCULATION
	//Real and imaginary values divided by number of samples to calculate average values, complex quantities calculated using X a Y swapped in order to compensate phase shift caused by inverting voltage and sensing opamps
	ut_CplxInitByRI(&VM10Cplx, qfp_fdiv(VM10R_Y, MEAS_READSPERMEASPERIOD), qfp_fdiv(VM10R_X, MEAS_READSPERMEASPERIOD));
	ut_CplxInitByRI(&VM100Cplx, qfp_fdiv(VM100R_Y, MEAS_READSPERMEASPERIOD), qfp_fdiv(VM100R_X, MEAS_READSPERMEASPERIOD));
	ut_CplxInitByRI(&IMCplx, qfp_fdiv(IM_Y, MEAS_READSPERMEASPERIOD), qfp_fdiv(IM_X, MEAS_READSPERMEASPERIOD));

	//IMPEDANCE CALCULATION
	//Minor hack to allow to scale impedance Z to physical units [Ohms], but also to keep voltage and current measurement in ADC counts
	VM10Cplx.Magnitude = qfp_fdiv(VM10Cplx.Magnitude, CAL_R10RGAIN);
	VM100Cplx.Magnitude = qfp_fdiv(VM100Cplx.Magnitude, CAL_R100RGAIN);
	IMCplx.Magnitude = qfp_fdiv(IMCplx.Magnitude, CAL_IMGAIN);

	ut_CplxDiv(&Z10Cplx, &VM10Cplx, &IMCplx);		//Impedance calculation Z = V / I;
	ut_CplxDiv(&Z100Cplx, &VM100Cplx, &IMCplx);		//Impedance calculation Z = V / I;

	VM10Cplx.Magnitude = qfp_fmul(VM10Cplx.Magnitude, CAL_R10RGAIN);
	VM100Cplx.Magnitude = qfp_fmul(VM100Cplx.Magnitude, CAL_R100RGAIN);
	IMCplx.Magnitude = qfp_fmul(IMCplx.Magnitude, CAL_IMGAIN);

	//SHORT CIRCUIT AND IMPEDANCE SLOPE COMPENSATION
	//ut_CplxInitByRI(&Z10CompCplx, Z10Cplx.Real - CAL_SHORT_RS, Z10Cplx.Imaginary - CAL_SHORT_XS);
	//ut_CplxInitByRI(&Z100CompCplx, Z100Cplx.Real - CAL_SHORT_RS, Z100Cplx.Imaginary - CAL_SHORT_XS);

	ut_CplxInitByRI(&Z10CompCplx, qfp_fmul(qfp_fsub(Z10Cplx.Real, CAL_Z10SHORT_RS), CAL_Z10SLOPECOMP), qfp_fmul(Z10Cplx.Imaginary - CAL_Z10SHORT_XS, CAL_Z10SLOPECOMP));
	ut_CplxInitByRI(&Z100CompCplx, qfp_fmul(qfp_fsub(Z100Cplx.Real, CAL_Z100SHORT_RS), CAL_Z100SLOPECOMP), qfp_fmul(Z100Cplx.Imaginary - CAL_Z100SHORT_XS, CAL_Z100SLOPECOMP));

	//Additional calculations with reactance and resistance
	float LocDF10R = qfp_fdiv(Z10CompCplx.Real, Z10CompCplx.Imaginary);
	float LocDF100R = qfp_fdiv(Z100CompCplx.Real, Z100CompCplx.Imaginary);
	float LocQ10R = qfp_fdiv(Z10CompCplx.Imaginary, Z10CompCplx.Real);
	float LocQ100R = qfp_fdiv(Z100CompCplx.Imaginary, Z100CompCplx.Real);

	float LocC10R = qfp_fdiv( -1, qfp_fmul( M_TWOPI, qfp_fmul(gen_GetActualFreq(), Z10CompCplx.Imaginary) ) );
	float LocC100R = qfp_fdiv( -1, qfp_fmul( M_TWOPI, qfp_fmul(gen_GetActualFreq(), Z100CompCplx.Imaginary) ) );
	float LocL10R = qfp_fdiv(Z10CompCplx.Imaginary, qfp_fmul(M_TWOPI, gen_GetActualFreq()));
	float LocL100R = qfp_fdiv(Z100CompCplx.Imaginary, qfp_fmul(M_TWOPI, gen_GetActualFreq()));


	//STATISTICS
	ut_CalculateOnlineMeanStdev(VM10Cplx.Magnitude, &VM10R);
	ut_CalculateOnlineMeanStdev(VM10Cplx.Angle, &VMFi10R);
	ut_CalculateOnlineMeanStdev(VM100Cplx.Magnitude, &VM100R);
	ut_CalculateOnlineMeanStdev(VM100Cplx.Angle, &VMFi100R);
	ut_CalculateOnlineMeanStdev(IMCplx.Magnitude, &IM);
	ut_CalculateOnlineMeanStdev(IMCplx.Angle, &IMFi);

	ut_CalculateOnlineMeanStdev(Z10CompCplx.Magnitude, &Z10R);
	ut_CalculateOnlineMeanStdev(Z10CompCplx.Angle, &ZFi10R);
	ut_CalculateOnlineMeanStdev(Z10CompCplx.Real, &R10R);
	ut_CalculateOnlineMeanStdev(Z10CompCplx.Imaginary, &X10R);
	ut_CalculateOnlineMeanStdev(Z100CompCplx.Magnitude, &Z100R);
	ut_CalculateOnlineMeanStdev(Z100CompCplx.Angle, &ZFi100R);
	ut_CalculateOnlineMeanStdev(Z100CompCplx.Real, &R100R);
	ut_CalculateOnlineMeanStdev(Z100CompCplx.Imaginary, &X100R);

	ut_CalculateOnlineMeanStdev(LocDF10R, &DF10R);
	ut_CalculateOnlineMeanStdev(LocQ10R, &Q10R);
	ut_CalculateOnlineMeanStdev(LocC10R, &CS10R);
	ut_CalculateOnlineMeanStdev(LocL10R, &LS10R);
	ut_CalculateOnlineMeanStdev(LocDF100R, &DF100R);
	ut_CalculateOnlineMeanStdev(LocQ100R, &Q100R);
	ut_CalculateOnlineMeanStdev(LocC100R, &CS100R);
	ut_CalculateOnlineMeanStdev(LocL100R, &LS100R);

	ut_CalculateOnlineMeanStdev(FMI[CLIPVM10LOW], &Clip10RLow);
	ut_CalculateOnlineMeanStdev(FMI[CLIPVM10HIGH], &Clip10RHigh);
	ut_CalculateOnlineMeanStdev(FMI[CLIPVM100LOW], &Clip100RLow);
	ut_CalculateOnlineMeanStdev(FMI[CLIPVM100HIGH], &Clip100RHigh);
	ut_CalculateOnlineMeanStdev(FMI[CLIPIMLOW], &ClipIMLow);
	ut_CalculateOnlineMeanStdev(FMI[CLIPIMHIGH], &ClipIMHigh);

	LatestEvaluatedCycle = FinishedCycles;
}

/*
 * Start the measurement with specified frequency fm [Hz] and number of measurement cycles for statistics
 * Before actual measurement is started, dropCycles number of cycles is performed and results dropped to stabilize the reading
 * Number of actually evaluated cycles = desiredcycles - dropcycles; desiredcycles must be higher than dropcycles
 */
err_Td sd_HandleMeasurementSync(float fm_Hz, uint16_t desiredCycles, uint8_t dropCycles){
	if( ( fm_Hz < SD_FMMIN_HZ ) || ( fm_Hz > SD_FMMAX_HZ ) ) return err_Td_Range;
	if(!desiredCycles) return err_Td_NotValid;

	MeasurementActiveFlag = 1;
	FinishedCycles = 0;
	DesiredCycles = desiredCycles;
	DropCycles = dropCycles;
	LatestEvaluatedCycle = 0;

	gen_Prepare(fm_Hz);
	meas_Prepare(fm_Hz, 30);
	meas_SetCycleFinishedHook(MeasurementCycleFinishedHook);

	gen_Start();
	meas_Start();

	Freq_Hz = gen_GetActualFreq();

	while(1){
		if( LatestEvaluatedCycle != FinishedCycles ){

			EvaluateMeasurement( (DropCycles-- > 0) );

			if(FinishedCycles < DesiredCycles) meas_Continue();
			else break;
		}
	}

	UpdateFMIandFreq(&Res10R);
	UpdateFMIandFreq(&Res100R);

	sd_StopMeasurement();

	return err_Td_Ok;
}


/*
 * Check whether measurement is active (1) or not (0)
 */
uint8_t sd_GetMeasurementStatus(void){
	return MeasurementActiveFlag;
}


/*
 * Returns optimal range (0 = measurement not valid)
 */
static sd_Results* sd_GetOptimalRange(void){
	if( qfp_fadd(qfp_fdiv(Clip10RLow.Mean, Clip10RLow.Cnt), qfp_fdiv(Clip10RHigh.Mean, Clip10RHigh.Cnt) ) < CLIPTRESHOLDRATIO ){
		return &Res10R;
	}
	else if( qfp_fadd( qfp_fdiv(Clip100RLow.Mean, Clip100RLow.Cnt), qfp_fdiv(Clip100RHigh.Mean, Clip100RHigh.Cnt) ) < CLIPTRESHOLDRATIO){
		return &Res100R;
	}
	else{
		return 0;
	}
}


/*
 * Returns measurement results, more suitable voltage range is selected
 * Returns optimal range - see sd_GetOptimalRange
 */
sd_Results* sd_GetMeasurementResults(void){
	return sd_GetOptimalRange();
}

/*
 * Returns actual measurement frequency as indicated by generator module during measurement start
 */
float sd_GetMeasFreq(void){
	return Freq_Hz;
}













/*
#define SD_XCOVTRESHOLD	0.333f
#define SD_ERRORIMPEDANCETRESHOLD_R	10000
int8_t sd_ProposeFrequencyUpdateDirection(sd_Results* r){
	if(r==0) return 0;

	if( qfp_float2int(r->Z->Mean) > SD_ERRORIMPEDANCETRESHOLD_R ) return 0;		//Measured impedance is to far out of range




	float CoV = qfp_fdiv(r->X->Stdev, r->X->Mean);
	if(CoV < 0.0f) CoV = qfp_fmul(-1.0f, CoV);									//Make CoV positive
	if( CoV > SD_XCOVTRESHOLD ) return 0;										//Reactive part too noisy, possibly resistive load, not possible to decide if inductive or capacitive load so not possible to recommend frequency change



	if( r->X->Mean < 0.0 ) return 1;	//Capacitive load, proposed to

	{

	}
	else{	//Inductive load

	}

}
*/
